/*
 * PathPlanner.h
 *
 *  Created on: May 12, 2016
 *      Author: throne
 */

#ifndef PX4_OPERATOR_SRC_PATH_PLANNER_PATHPLANNER_H_
#define PX4_OPERATOR_SRC_PATH_PLANNER_PATHPLANNER_H_
#include <math.h>
#include <string>
#include <ros/ros.h>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>
using namespace Eigen;
using namespace std;



class PathPlanner {
public:
enum ACTION_TYPE
{
	INVALID = 0,
	EXIT,
	START,
	STAY,
	MOVER,
	MOVETO,
	ROTATETO,
	ROTATER,
	MOVEANDROTTO,
	ARM,
	DISARM,
};
struct SetPoint
{
	double x;
	double y;
	double z;
	double yaw;
};
public:
	SetPoint setpoint;
	SetPoint prev_target;
	SetPoint target;
	double time_left;
	double time_duration;

public:
	void TakeActionScript(ACTION_TYPE type, double p0=0, double p1=0, double p2=0, double p3=0, double p4=0);
	void TakcActionFromString(const char* str);
	double Step(double dt);
	Affine3d GetSetPointEigen();


private:
	bool charIsValid(char c);
	ACTION_TYPE str_to_action(const char* str);

public:
	PathPlanner();
	virtual ~PathPlanner();
};

#endif /* PX4_OPERATOR_SRC_PATH_PLANNER_PATHPLANNER_H_ */
